/*
 * Copyright (C) 2017 - 2019, AEye, Inc.
 * All rights reserved.
 */
#include <cstring>
#include <memory>

#include "spdlog/spdlog.h"

#include "AEyeSensorAPI.h"
#include "SocketDiagnosticReader.hpp"
#include "Utils.hpp"

namespace aeye {


SocketDiagnosticReader::SocketDiagnosticReader(const std::string &sensorAddr, uint16_t clientPort)
: reader_(sensorAddr, clientPort, DEFAULT_DIAGNOSTIC_PORT, false), pcap_queue_manager_(isRunning, maxQueueSize),
  frame_queue_manager_(isRunning, maxQueueSize)
{ }

SocketDiagnosticReader::~SocketDiagnosticReader() noexcept {
  spdlog::trace("SDR dtor called.");
  if (isRunning) stop();
}

void SocketDiagnosticReader::run() {
  spdlog::trace("Started running socketDiagnosticReader.");
  set_thread_name(AEYE_LIT("DiagnosticRun"));
  isRunning = true;

  auto buf = std::make_unique<uint8_t[]>(UDP_BUF_LEN);

  // Datagram source
  sockaddr_in src_addr{};

  frame_queue_manager_.clean();
  pcap_queue_manager_.clean();
  PacketDiagnosticData bufferReturn{};
  bool mismatch_error = false;

  while (isRunning) {
    if (pcapModeOn) {
      std::vector<uint8_t> pcapPacket = pcap_queue_manager_.pop();
      std::memcpy(&bufferReturn, pcapPacket.data(), pcapPacket.size());
      DiagnosticData data{bufferReturn.body};
      frame_queue_manager_.push(std::move(data));
      continue;
    }
    fd_set readFlags{};

    if (!reader_.run_select(3, (30 * 1000), readFlags)) {
      continue;
    }

    const size_t bytesReceived = reader_.receive_bytes(buf.get(), src_addr);

    if (bytesReceived == 0) {
      continue;
    }

    if (!reader_.is_set(readFlags) || !isRunning) {
      // Timeout or wrong checksum
      continue;
    }

    if (!reader_.validate_ip_addr(src_addr)) {
      continue;
    }

    // non-UB way of creating struct from bytes
    std::memcpy(&bufferReturn, buf.get(), sizeof(bufferReturn));

    if (!Reader::find_marker(bufferReturn.header.marker)) {
      spdlog::warn("Encountered invalid marker in SDR.");
      continue;
    }
    if (bufferReturn.header.type != AEyeCommands::RET_DIAG_DATA) {
      spdlog::warn("Encountered invalid type `{}` in SDR.", uint16_t{bufferReturn.header.type});
      continue;
    }
    // Do a version check
    if (bufferReturn.header.version!=RET_DIAG_VERSION) {
      if (!mismatch_error) {
        spdlog::warn("Unsupported version of Diagnostic packet received. Expecting version {}.", RET_DIAG_VERSION);
        spdlog::warn("Diagnostics will be disabled due to a client/service mismatch. Please update sensor and client "
                     "accordingly to regain usability.");
        mismatch_error = true;
      }
      continue;
    }
    if (!pcapModeOn) frame_queue_manager_.push(DiagnosticData{bufferReturn.body});
  }
}

void SocketDiagnosticReader::start() {
  reader_.start();
}

DiagnosticData SocketDiagnosticReader::pop() {
  return frame_queue_manager_.pop();
}

void SocketDiagnosticReader::pushPcapPacket(std::vector<uint8_t> &&data) {
  pcap_queue_manager_.push(std::move(data));
}

void SocketDiagnosticReader::stop() noexcept {
  if (!isRunning) return;
  isRunning = false;
  reader_.stop();
  spdlog::trace("Stopped SDR.");
}

void SocketDiagnosticReader::setPcapMode(bool value) noexcept {
  pcapModeOn = value;
}

}